REAL dcut(REAL d, int i) {

	int sft = __SFT - i; 
	UINT id = ((*(UINT*)&d)>>sft)<<sft;
	return *(REAL*)&id;

}

void resample(P &p) {
	
	do {
		
		p.Vel[0] = p.v_esc*(2*drand48() - 1);
		p.Vel[1] = p.v_esc*(2*drand48() - 1);
		p.Vel[2] = p.v_esc*(2*drand48() - 1);
		
	} while ( SQR(p.v_esc) < SQR(p.Vel[0])+SQR(p.Vel[1])+SQR(p.Vel[2]) );
	
}

template <typename T>
void swap(T &a, T &b) {
	T tmp = a;
	a = b;
	b = tmp;
}

void find_vel() {
			
	
	

	
			printf("\nfind_vel() ...\n"); 
			
			dg.update_derivatives();
		
			printf("\tTry to allocate %g MBs for rho\n", dg.n*n*sizeof(TRHO)/1024.0/1024.0 );
			TRHO *m = new TRHO[dg.n*n];
			ms = new REAL[dg.n];

			TRHO *mjjx = new TRHO[8*dg.n];
			TRHO *mjjy = new TRHO[8*dg.n];
			TRHO *mjjz = new TRHO[8*dg.n];
			
			Vector<REAL>  *GV = new Vector<REAL>[n];
			Vector<REAL>  *L  = new Vector<REAL>[n];
			
			REAL *dm = new REAL[dg.n];			
			
			FILE *fp_fit = _fopen("fit.log", resumeF ? "a" : "w");
			
			bool firstTime = true;
			
			int _n(n);
			
			for (int j=0; j<_n; j++)
				if (!ag.isInGrid(p[j].Pos))
					swap(p[j--], p[--_n]);
			
			printf("_n = %i\n", _n);
			
			
			dh_min = dcut(dh_min, sft);//pow(base, int(log(0.2)/log(base))-1); //pow(base,-32); // min time step 
			dh_max = dcut(dh_max, sft);//pow(base,+4); // max time step
			
			
			if (h==0) for (int j=0; j<n; j++) { 
				p[j].h = 0;  p[j].dh = dh_min;
			}
			
			int n_act(n); 
			int *j_act = new int[n]; // list of active particles 
			// at start all particles are active
			for (int j=0; j<n; j++) j_act[j]= j;
			
			
			int *n_dh = new int[256]; n_dh+=128;
			
			Timer tr, tr_orb; 
			tr.set(); 
			tr_orb.set(); tr_orb.pause();

			printf("\n");
			double ss(0);
			tr_orb.resume();
			int nn=4;
			bool failflag[8]; memset(failflag, 0, 8*sizeof(bool));
			int n_resampled(0);
			Timer timer_tmp;
			timer_tmp.set();
			for (int _j=0; _j<n_act; _j+=nn) {

				int _nn = n_act-_j<nn ? n_act-_j : nn;
				calc_orb( p, m, &j_act[_j], _nn, failflag  );
				
				//if (_j>52) exit(0);
				//printf("j = %i\n", _j);
				/*
				int j = j_act[_j]; 
				double s(0);
				for (int i=0; i<dg.n; i++)  
					s += rho[j*dg.n+i];
				
				ss+=s;
				*/
				/*
				for (int jj=0; jj<_nn; jj++ ) {
					int j = j_act[_j+jj];
					printf( "%4i %4i   %10.7f %10.7f  %6.2f %6.2f   %4i \n", j, _j+jj, s, ss, MOD3(p[j].Vel), p[j].v_esc, n_resampled );
				}
				*/
				
				vector <int> A, B;
				for (int jj=0; jj<_nn; jj++ )
					!failflag[jj] ? A.push_back(j_act[_j+jj]) : B.push_back(j_act[_j+jj]);
					
				vector <int> AB; AB = A;
				AB.insert(AB.end(), B.begin(), B.end());
				
				for (int jj=0; jj<_nn; jj++ )  
					j_act[_j+jj] = AB[jj];
				
				for (int jj=0; jj<B.size(); jj++ ) {
				
					//printf("halo.rmax = %7.4f\n", halo.rmax);
					//printf("p[%i].v_esc = %7.4f\n", B[jj], p[B[jj]].v_esc);
					//printf("%i: resampled (%7.4f  %7.4e) => ", B[jj], MOD3(p[B[jj]].Pos), MOD3(p[B[jj]].Vel));
					resample(p[B[jj]]);
					//printf("(%7.4f  %7.4e)\n", B[jj], MOD3(p[B[jj]].Pos), MOD3(p[B[jj]].Vel));
				
					n_resampled++;
					
				}
				
				_j-=B.size();
				//exit(0);
				//if (220<j) break;
			}
			
			timer_tmp.get();
			timer_tmp.print();
			
			printf("time = %g\n", timer_tmp.sin);
			
			//exit(0);
			printf("\n");
			printf("n_resampled = %i\n", n_resampled);
		
			tr_orb.pause();
			
			
			
			REAL mtot0(0);
			for (int i=0; i<dg.n; i++) mtot0+=m0[i];
			REAL mtot02 = SQR(mtot0);
			printf("mtot0 = %g\n", mtot0);
			
		



			vector< vector<int> > map;
			dg.makeAMRGrid(map, m0);
			
			
			REAL  *_m0 = new REAL[dg._n];
			REAL  *_ms = new REAL[dg._n];
			REAL  *_dm = new REAL[dg._n];
			
			REAL *_mjjx = new REAL[8*dg._n];
			REAL *_mjjy = new REAL[8*dg._n];
			REAL *_mjjz = new REAL[8*dg._n];
			
			REAL *_m = new REAL[8*dg._n];
			
			dg.mapAMRGrid(_m0, map, m0);
			saveAMRGrid("_m0.halo.dat", map, m0);

			for(int i=0; i<dg._n; i++ )
				printf("_m0[%i] = %g\n", i, _m0[i]);
			
			
			mtot0 = 0;
			for(int i=0; i<dg._n; i++ )
				mtot0+=_m0[i];
			printf("mtot0 = %g\n", mtot0);
			
			
			// mass normalization
			for (int j=0; j<_n; j++)
			p[j].Mass = mtot0/_n;
				
				

			
			do {
				
				// calc_rhos
				memset(ms, 0, dg.n*sizeof(*ms));
				for (int j=0; j<n; j++)
				for (int i=0; i<dg.n; i++)  
					ms[i] += m[j*dg.n+i];
				
				
				//if (stype==0) for (int i=0; i<dg.n; i++) drho_rel[i] = rhos[i]-rho0[i];
				//if (stype==1) for (int i=0; i<dg.n; i++) drho_rel[i] = rhos[i]!=0 ? (sqrt(rhos[i])-sqrt(rho0[i]))/sqrt(rhos[i]) : 0;	
				//if (stype==2) for (int i=0; i<dg.n; i++) drho_rel[i] = rhos[i]-rho0[i] !=0 ? (rhos[i]-rho0[i])/sqrt(pow(rhos[i]-rho0[i], 2)) : 0;
				dg.mapAMRGrid(_ms, map, ms);
				
				for (int i=0; i<dg._n; i++) _dm[i] = _ms[i]-_m0[i] !=0 ? (_ms[i]-_m0[i])/sqrt(pow(_ms[i]-_m0[i], 2)) : 0;	
				
	
				
				if (step%step_out==0) {	
					
					save(i_out);		
					
					printf("out: %g %i\n", h, step);
					
					char fname[256];
					
					if (trackF) {
						sprintf(fname, "track/track.%05i.dat", i_out);
						save_track_float(fname);
					}
					
					if (halo.n) { 
						calc_w(w);
						char fname[256];
						sprintf(fname, "%s/w.%05i.dat", ssfolder, i_out);
						save_w(w, fname);
					}
					
					
					sprintf(fname, "vel/vel.%05i.dat", i_out);
					save_vel(fname);
	
					sprintf(fname, "ms/ms.%05i.dat", i_out);
					save_arr(fname, ms, dg.n);
					
					{
						float *_dm = new float[dg.n];
						for (int i=0; i<map.size(); i++ ) {
							double m0i(0), msi(0);
							for (int j=0; j<map[i].size(); j++ )  m0i+= m0[map[i][j]];
							for (int j=0; j<map[i].size(); j++ )  msi+= ms[map[i][j]];
							for (int j=0; j<map[i].size(); j++ ) _dm[map[i][j]] = msi-m0i;
						}
						
						sprintf(fname, "_dm/_dm.%05i.dat", i_out);
						save_arr(fname, _dm, dg.n);
						delete [] _dm;
					}
					
					//sprintf(fname, "dm/dm.%05i.dat", i_out);
					//save_arr(fname, dm, dg.n);

					
					i_out++;
				}

				
				
				// calc s 
				REAL s0(0), s1(0), s2(0);
				for (int i=0; i<dg._n; i++) {
					s0+= pow(_ms[i]-_m0[i], 2);
					s1+= pow(sqrt(_ms[i])-sqrt(_m0[i]), 2);
					s2+= fabs(_ms[i]-_m0[i]);
				}
				s0=sqrt(s0*dg._n)/2;
				s1/=2;
				s2/=2;
				
				// calc sw 
				calc_w(w);
				REAL sw(0);
				for (int i=0; i<sg.n; i++) {
					REAL swi(0);
					for (int k=0; k<vg.n; k++)
						swi+= pow(w[i*vg.n+k]-wa[i*vg.n+k], 2);
					sw+=swi;
				}
				sw/=sg.n*vg.n;

				
				REAL sGV(0);
				for (int j=0; j<n; j++)
				sGV+= GV[j].sqr();
				sGV = sqrt(sGV/n);
				
				
				tr.get();					
				tr_orb.resume(); tr_orb.get();
							        // 1      2    3       4    5         6     7     8       9     10      11  12
				fprintf(stdout, "\n%5i  %6.4f %6.5f   %6.3f %6.3f   %7.2e %7.2e %9.7e  %7.3e %7.3e   %5i %u\n\n", 
						step,  h, dh, tr.sin,tr.sin_tot,  s0,s1,s2, sw,sGV,   n_act, n_resampled); fflush(stdout);
						 //1   2   3    4      5          6  7  8    9  10    11       12    
				fprintf(fp_fit, "%5i  %7.5e %8.5e   %7.5e %8.5e   %7.5e %7.5e %7.5e %7.5e %7.5e   %5i %u\n", 
						step,  h, dh, tr.sin,tr.sin_tot,  s0,s1,s2, sw,sGV,   n_act, n_resampled); fflush(fp_fit);
						  
				tr_orb.set(); tr_orb.pause();				
				tr.set();
				//exit(0);
				// optimization

				P pjj[3][8];	
				bool failflag[3][8]; 
				Vector<REAL> DV[3][8];
				sGV = 0;
				
				for ( int _j=0; _j<n_act; _j+=nn ) {
					
					int _nn = n_act-_j<nn ? n_act-_j : nn;
									
					for ( int jj=0; jj<_nn; jj++ ) {

						int j = j_act[_j+jj];
						REAL dv = dvk_mu*p[j].v_esc; // > 0.1%
						
						int n_try(0);
						
						do {
						
							DV[0][jj] = Vector<REAL>(dv,0,0);                                                                            
							DV[1][jj] = Vector<REAL>(0,dv,0);
							DV[2][jj] = Vector<REAL>(0, 0,dv);

							
							#ifdef BASISWIND_FIX
							Vector<REAL> X; X.rnd(); 
							DV[0][jj].rnd(dv);
							DV[1][jj] = dv*(DV[0][jj]%X).ort();
							DV[2][jj] = dv*(DV[0][jj]%DV[1][jj]).ort();
							#endif
							
							
							for (int kk=0; kk<3; kk++ ) {
								pjj[kk][jj] = p[j];
								pjj[kk][jj].Vel[0] += DV[kk][jj].x;
								pjj[kk][jj].Vel[1] += DV[kk][jj].y;
								pjj[kk][jj].Vel[2] += DV[kk][jj].z;
							}
						
							if (100<n_try) { 
								
								do {
									resample(p[j]);
									calc_orb( p, m, &j_act[_j], _nn, *failflag);
									p[j].dh = dh_min;
								} while((*failflag)[jj]);
								
								jj--; 
								n_resampled++;
								break; 
							}
							
							n_try++;
							
						} while ( p[j].v_esc < MOD3(pjj[0][jj].Vel) ||  
									 p[j].v_esc < MOD3(pjj[1][jj].Vel) || 
									 p[j].v_esc < MOD3(pjj[2][jj].Vel) );
					}
					
	
					int J[] = {0,1,2,3,4,5,6,7};
					tr_orb.resume();
					calc_orb( pjj[0], mjjx, J, _nn, failflag[0]);
					calc_orb( pjj[1], mjjy, J, _nn, failflag[1]);
					calc_orb( pjj[2], mjjz, J, _nn, failflag[2]);
					tr_orb.pause();
				

					//mapping
					for ( int jj=0; jj< _nn; jj++ ) {
						
						dg.mapAMRGrid( &_m[jj*dg._n], map, &m[j_act[_j+jj]*dg.n]);
						
						dg.mapAMRGrid( &_mjjx[jj*dg._n], map, &mjjx[jj*dg.n]);
						dg.mapAMRGrid( &_mjjy[jj*dg._n], map, &mjjy[jj*dg.n]);
						dg.mapAMRGrid( &_mjjz[jj*dg._n], map, &mjjz[jj*dg.n]);
					}
					
					
					REAL dotsjj[3][8];
					for (int kk=0; kk<3; kk++)
					memset(dotsjj[kk], 0, 8*sizeof(REAL));

					
					for (int i=0; i<dg._n; i++)
					for ( int jj=0; jj< _nn; jj++ ) {
						
						dotsjj[0][jj] += _dm[i]*(_mjjx[jj*dg._n+i]-_m[jj*dg._n+i]); 
						dotsjj[1][jj] += _dm[i]*(_mjjy[jj*dg._n+i]-_m[jj*dg._n+i]);
						dotsjj[2][jj] += _dm[i]*(_mjjz[jj*dg._n+i]-_m[jj*dg._n+i]);
						
					}

					
					for ( int jj=0; jj< _nn; jj++ ) {
						
						int j = j_act[_j+jj];
						
						REAL dv = dvk_mu*p[j].v_esc;
						
						GV[j] = n*(dg._n/mtot0)*(2.0/dv)*(	dotsjj[0][jj]*DV[0][jj].ort() + 
																		dotsjj[1][jj]*DV[1][jj].ort() + 
																		dotsjj[2][jj]*DV[2][jj].ort()	);
						
						#ifdef DV_DH
						REAL dh_new = dvh_mu*p[j].v_esc/GV[j].module();
						#endif
					
						#ifdef DV_DH2
						REAL dh_new = sqrt(2*dvh_mu*p[j].v_esc/GV[j].module());
						#endif
					
						dh_new = dcut(dh_new, sft);

						if (dh_new < dh_min) dh_new = dh_min;
						if (dh_max < dh_new) dh_new = dh_max;

						p[j].dh = dh_new;
		
					}
					
					
					//resampling
					vector <int> A, B;
					for (int jj=0; jj<_nn; jj++ )
						if (!failflag[0][jj] && !failflag[1][jj] && !failflag[2][jj])
							A.push_back(j_act[_j+jj]); else B.push_back(j_act[_j+jj]);
					
					vector <int> AB; AB = A;
					AB.insert(AB.end(), B.begin(), B.end());
				
					
					for (int jj=0; jj<B.size(); jj++ ) {
						resample(p[B[jj]]);
						n_resampled++;
					}

					if (0 < B.size()) do {
						
						calc_orb( p, m, &j_act[_j], _nn, *failflag);
						
						for (int jj=0; jj<_nn; jj++ )  
							if ((*failflag)[jj] ) {
								resample(p[j_act[_j+jj]]);
								n_resampled++;
							}
							
						for (int jj=0; jj<_nn; jj++ )  
							if ((*failflag)[jj]) continue;
							
					} while(false);
					
					for (int jj=0; jj<_nn; jj++ )  
						j_act[_j+jj] = AB[jj];
					
					_j-=B.size();
				}
				
				
				sGV = 0;
				for (int j=0; j<_n; j++)
				sGV+= GV[j].sqr();
				sGV = sqrt(sGV/_n);
				
				printf("sGV = %.7e   %i\n", sGV, n_resampled);
				
						//exit(0);
				
				// shift active particles 
				for (int _j=0; _j<n_act; _j+=nn) {  
									
					bool failflag[8]; 
				
					int _nn = n_act-_j<nn ? n_act-_j : nn;
				
					for ( int jj=0; jj< _nn; jj++ ) {
					
						int j = j_act[_j+jj];
					
						#ifdef DV_DH
						Vector<REAL> dVelj = -1.0*GV[j]*p[j].dh;
						#endif
					
						#ifdef DV_DH2
						Vector<REAL> dVelj = -1.0*GV[j]*SQR(p[j].dh);
						#endif
					
						p[j].Vel[0] += dVelj.x;
						p[j].Vel[1] += dVelj.y;
						p[j].Vel[2] += dVelj.z;
						
						if (p[j].v_esc < MOD3(p[j].Vel)) {
							resample(p[j]);
							n_resampled++;
							p[j].dh = dh_min;
							GV[j]*=0;
						}
						
					}	
					
					calc_orb( p, m, &j_act[_j], _nn, failflag);
					
					vector <int> A, B;
					for (int jj=0; jj<_nn; jj++ )
					!failflag[jj] ? A.push_back(j_act[_j+jj]) : B.push_back(j_act[_j+jj]);
					
					vector <int> AB; AB = A;
					AB.insert(AB.end(), B.begin(), B.end());
				
					for (int jj=0; jj<_nn; jj++ )  
					j_act[_j+jj] = AB[jj];
				
					for (int jj=0; jj<B.size(); jj++ ) {
						resample(p[B[jj]]);
						p[B[jj]].dh = dh_min;
						GV[B[jj]]*=0;
						n_resampled++;
					}
					
					_j-=B.size();
				
					
				}
				

				
				int __sft1(0), __sft2(0);
				if (sizeof(REAL)==sizeof(float))   {  __sft1 = 23; __sft2 =  7;  } 
				if (sizeof(REAL)==sizeof(double))  {  __sft1 = 52; __sft2 = 10; } 
				
				
				// update n(dh) (deferential)
				memset(&n_dh[-128], 0, 4*256);
				for (int j=0;j<_n; j++) {
					
					int i = ((*(UINT*)&p[j].dh)>>(__sft1-sft)) - (1<<(__sft2+sft)) + (1<<sft);
					n_dh[i]++;
				}
				
				// list n(dh)
				for (int i=-128; i<+128; i++) {
					UINT idh = UINT(i + (1<<(__sft2+sft)) - (1<<sft)) <<(__sft1-sft);
					if (n_dh[i]) printf("\tn_dh[% 3i] = %5i  %g\n", i, n_dh[i], *(REAL*)&idh);
				}
				
				
				// find min incremaent to the global time 
				dh = dh_max;
				for (int j=0; j<_n; j++)
				if (p[j].h+p[j].dh-h < dh )
					dh = p[j].h+p[j].dh-h;
				
				printf("dh = %g\n", dh);
				
				// update list of active particles
				n_act = 0;
				for (int j=0; j<_n; j++)
					if (p[j].h+p[j].dh == h+dh)
						j_act[n_act++]=j;

				for (int _j=0, j; _j<n_act; _j++) {
					j = j_act[_j];
					p[j].h+=p[j].dh;	
				}	
				
				h+=dh;
				step++;
				printf("n_resampled = %i\n", n_resampled);
				/**/
				
				//exit(0);
			} while(step<nsteps);
			
			fclose(fp_fit);
			
			delete [] mjjx;
			delete [] mjjy;
			delete [] mjjz;
			
			delete [] ms;
			delete [] m;
			
			
			
}




